#ifndef SIMPLE_GEOM_ALGORITHM_HPP
#define SIMPLE_GEOM_ALGORITHM_HPP
#include "algorithm.h"

#include <functional>

#include "simple/support/function_utils.hpp"
#include "simple/support/algorithm/utils.hpp"

#include "bool_algebra.hpp"

namespace simple::geom
{

	template <typename Coordinate, size_t Dimensions, typename Order, typename Function, size_t LoopDimesntions>
	constexpr void loop_on
	(
		vector<Coordinate, Dimensions, Order>& index,
		const vector<Coordinate, Dimensions, Order>& lower,
		const vector<Coordinate, Dimensions, Order>& upper,
		const vector<Coordinate, Dimensions, Order>& step,
		Function&& callback
	)
	{
		if constexpr (LoopDimesntions == 0)
			support::invoke(std::forward<Function>(callback),index);
		else
		{
			constexpr size_t I = LoopDimesntions-1;
			for
			(
				index[I] = lower[I];
				index[I] < upper[I];
				index[I] += step[I]
			)
			{
				loop_on<Coordinate,Dimensions,Order,Function,I>
					(index, lower, upper, step,
					std::forward<Function>(callback));
			}
		}
	};

	template <typename Coordinate, size_t Dimensions, typename Order, typename Function>
	constexpr void loop
	(
		const vector<Coordinate, Dimensions, Order>& lower,
		const vector<Coordinate, Dimensions, Order>& upper,
		const vector<Coordinate, Dimensions, Order>& step,
		Function&& callback
	)
	{
		vector<Coordinate, Dimensions, Order> index{};
		loop_on(index, lower, upper, step,
			std::forward<Function>(callback));
	}

	template <typename Coordinate, size_t Dimensions, typename Order, typename Function>
	constexpr void loop
	(
		const vector<Coordinate, Dimensions, Order>& upper,
		Function&& callback
	)
	{
		using vector = vector<Coordinate, Dimensions, Order>;
		vector index{};
		loop_on(index, vector::zero(),  upper, vector::one(),
			std::forward<Function>(callback));
	}

	template <typename Coordinate, size_t Dimensions, typename Order, typename Function, size_t LoopDimesntions>
	constexpr void loop_on
	(
		vector<Coordinate, Dimensions, Order>& index,
		const support::range<vector<Coordinate, Dimensions, Order>>& bounds,
		const vector<Coordinate, Dimensions, Order>& step,
		Function&& callback
	)
	{
		loop_on(index, bounds.lower(), bounds.upper(), step,
			std::forward<Function>(callback));
	}

	template <typename Coordinate, size_t Dimensions, typename Order, typename Function>
	constexpr void loop
	(
		const support::range<vector<Coordinate, Dimensions, Order>>& bounds,
		const vector<Coordinate, Dimensions, Order>& step,
		Function&& callback
	)
	{
		vector<Coordinate, Dimensions, Order> index{};
		loop_on(index, bounds, step,
			std::forward<Function>(callback));
	}

	template <typename Coordinate, size_t Dimensions, typename Order, typename Function>
	constexpr void loop
	(
		const support::range<vector<Coordinate, Dimensions, Order>>& bounds,
		Function&& callback
	)
	{
		using vector = vector<Coordinate, Dimensions, Order>;
		vector index{};
		loop_on(index, bounds, vector::one(),
			std::forward<Function>(callback));
	}

	template <typename Coordinate,
		size_t Rows, size_t Columns,
		typename RowOrder, typename ColumnOrder
	>
	constexpr auto gauss_jordan_elimination
	(
		vector<
			vector<Coordinate, Columns, ColumnOrder>,
			Rows, RowOrder
		> m
	)
	{
		constexpr size_t D = std::min(Rows, Columns);
		for( size_t i = 0; i < D; ++i)
		{
			// TODO: make pivoting optional
			auto pivot = std::max_element(m.begin()+i, m.end(), [&i](auto a, auto b)
			{
				using support::abs;
				return abs(a[i]) < abs(b[i]);
			});
			support::swap(*pivot, m[i]);

			m[i] /= +m[i][i]; // ouch, easy to shoot foot here (+ makes a copy, otherwise disaster)
			for(size_t j = i+1; j < D; ++j)
				m[j] -=	m[i] * m[j][i];
			for(size_t j = i; j --> 0;)
				m[j] -=	m[i] * m[j][i];
		}
		return m;
	}

	template <typename Transform, typename Vector, typename AnotherCoord, typename Result>
	[[nodiscard]] constexpr Result
	deeply_transformed(const Vector& vector, Transform&& transform)
	{
		Result result{};
		loop(Vector::meta::template dimensions<>, [&](auto i)
		{
			result[i] = support::invoke(
				std::forward<Transform>(transform),
				vector[i]
			);
		});
		return result;
	}

	template <typename T, size_t D, typename O>
	[[nodiscard]] constexpr auto column(const vector<T,D,O>& one)
	{
		return one.transformed([](const auto &x) {return vector(x);}); // 1xD
	}

	template <typename T, size_t D, typename O>
	[[nodiscard]] constexpr vector<vector<T,D,O>, D,O> expand(const vector<T,D,O>& one, const vector<T,D,O>& other)
	{
		const auto row = vector(one); // Dx1
		const auto col = column(other); // 1xD
		return row(col); // Dx1 * 1xD = DxD
	}

	//FIXME: don't think this actually works for rotation and such
	template <typename T, size_t D, typename O, typename Ret>
		// Ret = vector<T, (D*D - D)/2 + 1>>
		// D*D all pairs
		// -D the diagonal
		// /2 pairing the rest (upper triangle  - lower triangle)
		// +1 for the accumulated diagonal which is the dot product
	[[nodiscard]] constexpr
	Ret dot_wedge(const vector<T,D,O>& one, const vector<T,D,O>& other)
	{
		const auto M = expand(one, other);

		Ret r{};

		// accumulate diagonal, which is the dot product
		for(size_t i = 0; i < D; ++i)
			r[Ret::dimensions - 1] += M[i][i];

		// upper triangle minus lower triangle gives wedge product (kind of)
		// some signs are flipped, but that's just a choice of basis right?
		// doesn't matter right? ... right?
		for(size_t i = 0, j = 0, k = 1; j < D-1; ++i)
		{
			r[i] = M[j][k] - M[k][j]; // was
			// r[Ret::dimensions - 1 - i] = M[j][k] - M[k][j]; // why should be ?

			++k;
			if(k == D)
			{
				++j;
				k = j+1;
			}
		}

		// for 4 dimensions will get 6 + 1, so to get quaternion
		// multiplication I'm guessing need to squash that six by another
		// factor of two with some other magic pairing
		// for 8 similar story 28 + 1, need to reduce by another factor of
		// 4 (two magic pairings, first hopefully analogous to quaternions)
		// to get 7 + 1
		// huge if true...

		return r;
	}


} // namespace simple::geom

#endif /* end of include guard */
